/****************************************************************************
 *
 * 1 main.c BSP_CAN_Send_Loop() 发送模拟数据到电控
 *
 *
 *****************************************************************************/
#include "bsp_can_demo.h"

static void C620_SetCurrentsGroup(C620_Group_t group, int16_t currents[]);
static void C620_SendMotorStatus(uint32_t motor_id, int16_t angle, int16_t speed, int16_t current, int8_t temp);

// 全局变量定义
C620_Feedback_t motor_feedback[8]; //   四个电机的反馈数据结构体
// CAN消息结构体定义
CAN_TxHeaderTypeDef TxHeader;
CAN_RxHeaderTypeDef RxHeader;
uint8_t TxData[8];  // 发送数据缓冲区 从主机发送到C620电调
uint8_t RxData[8];  // 接收数据缓冲区
uint32_t TxMailbox; // 发送邮箱

void BSP_CAN_Init(CAN_HandleTypeDef *hcan_p) // 传递CAN句柄初始化
{
    // 启动CAN滤波器（回环模式下可配置为不过滤）
    CAN_FilterTypeDef canFilter;
    canFilter.FilterBank = 0;                      // 滤波器组0
    canFilter.FilterMode = CAN_FILTERMODE_IDMASK;  // 掩码模式
    canFilter.FilterScale = CAN_FILTERSCALE_32BIT; // 32位模式
    canFilter.FilterIdHigh = 0x0000;               // ID高16位
    canFilter.FilterIdLow = 0x0000;                // ID低16位
    canFilter.FilterMaskIdHigh = 0x0000;           // 掩码高16位（全不过滤）
    canFilter.FilterMaskIdLow = 0x0000;            // 掩码低16位（全不过滤）
    canFilter.FilterFIFOAssignment = CAN_RX_FIFO0; // 分配到FIFO0
    canFilter.FilterActivation = ENABLE;           // 使能滤波器
    canFilter.SlaveStartFilterBank = 14;           // 从滤波器组

    HAL_CAN_ConfigFilter(hcan_p, &canFilter); // 下发滤波器参数

    // 启动CAN
    HAL_CAN_Start(hcan_p);

    // 启动CAN接收中断
    HAL_CAN_ActivateNotification(hcan_p, CAN_IT_RX_FIFO0_MSG_PENDING);
}

//发送一次电控数据到C620电调
void BSP_CAN_RM_Send_Loop()
{
    int16_t currents[4] = {500, -1000, -500, 1000};
    C620_SetCurrentsGroup(C620_GROUP_1_4, currents);
    HAL_Delay(100);
    C620_SetCurrentsGroup(C620_GROUP_5_8, currents);
}

/**
 * @brief 设置一组C620电机的电流值
 * @param group 电机组别：0表示ID1-4(使用0x200标识符)，1表示ID5-8(使用0x1FF标识符)
 * @param currents 电流值数组，包含4个元素，范围-16384~16384，对应-20A~20A
 * @note 该函数通过一条CAN报文同时控制4个电机，提高通信效率
 * @example
 *   int16_t currents[4] = {8192, -4096, 0, 4096}; // 10A, -5A, 0A, 5A
 *   C620_SetCurrentsGroup(0, currents); // 控制ID1-4电机
 */
static void C620_SetCurrentsGroup(C620_Group_t group, int16_t currents[])
{
    CAN_TxHeaderTypeDef header;
    uint8_t data[8] = {0};
    uint32_t mailbox;

    // group: 0 -> ID1-4 (0x200), 1 -> ID5-8 (0x1FF)
    header.StdId = (group == 0) ? 0x200 : 0x1FF;
    header.ExtId = 0;
    header.RTR = CAN_RTR_DATA;
    header.IDE = CAN_ID_STD;
    header.DLC = 8;
    header.TransmitGlobalTime = DISABLE;

    for (int i = 0; i < 4; i++)
    {
        data[i * 2] = (currents[i] >> 8) & 0xFF; // 高字节
        data[i * 2 + 1] = currents[i] & 0xFF;    // 低字节
    }
    HAL_GPIO_TogglePin(LED0_GPIO_Port, LED0_Pin);
    HAL_CAN_AddTxMessage(&hcan, &header, data, &mailbox);
}

// 模拟8个C620电调发送数据到主机，
void BSP_CAN_MotorC620_Send_Loop()
{
    int16_t angle = 0;
    int16_t speed = 0;
    int16_t current = 0;
    int8_t temperature = 0;

    for (int i = 0; i < 8; i++)
    {
        angle += 10;
        speed += 11;
        current += 12;
        temperature += 1;
        C620_SendMotorStatus(0x201 + i, angle, speed, current, temperature);
        HAL_Delay(50);
    }
}

/**
 * @brief 模拟电调发送模拟数据到主机
 * @param group 电机标识符：0X201~0X208
 * @param DATA[0]  转子自机械角度高8位 机械角度0~8191 对应 0~360°
 * @param DATA[1]  转子机械角度低8位
 * @param DATA[2]  转子转速高8位       RPM值
 * @param DATA[3]  转子转速低8位
 * @param DATA[4]  实际转矩电流高8位    可能是毫安
 * @param DATA[5]  转子转矩电流低8位
 * @param DATA[6]  电机温度            摄氏度
 * @param DATA[7]  NULL
 * @note 该函数通过一条CAN报文
 * @example
 */
static void C620_SendMotorStatus(uint32_t my_motor_id, int16_t angle, int16_t speed, int16_t current, int8_t temp)
{
    CAN_TxHeaderTypeDef header;
    uint8_t data[8] = {0};
    uint32_t mailbox;

    // motor_id 0x201~0x208
    header.StdId = my_motor_id;
    header.ExtId = 0;
    header.RTR = CAN_RTR_DATA;
    header.IDE = CAN_ID_STD;
    header.DLC = 8;
    header.TransmitGlobalTime = DISABLE;

    data[0] = (angle >> 8) & 0xFF;
    data[1] = angle & 0xFF;
    data[2] = (speed >> 8) & 0xFF;
    data[3] = speed & 0xFF;
    data[4] = (current >> 8) & 0xFF;
    data[5] = current & 0xFF;
    data[6] = temp;
    data[7] = 0;
    HAL_GPIO_TogglePin(LED0_GPIO_Port, LED0_Pin);
    HAL_CAN_AddTxMessage(&hcan, &header, data, &mailbox);
}

/**
 * @brief C620电调反馈数据解算函数
 * @param can_id CAN标识符 (0x201~0x208)
 * @param data 接收到的8字节数据
 * @param feedback 反馈数据结构体指针
 * @return uint8_t 解算成功的电机ID (1~8)，0表示解算失败
 */
static uint8_t C620_DecodeFeedback(uint32_t can_id, uint8_t data[8], C620_Feedback_t *feedback)
{
    // 检查标识符范围
    if ((can_id < 0x201 || can_id > 0x208))
    {
        return 0;
    }

    // 计算电机ID
    uint8_t motor_id = can_id - 0x200;

    // 解算转子机械角度 (0~8191 对应 0~360°)
    feedback->angle = (data[0] << 8) | data[1];

    // 解算转速 (rpm，有符号)
    feedback->speed = (int16_t)((data[2] << 8) | data[3]);

    // 解算实际转矩电流 (有符号，-16384~16384 对应 -20A~20A)
    feedback->current = (int16_t)((data[4] << 8) | data[5]);

    // 解算电机温度 (°C)
    feedback->temperature = data[6];

    // 计算角度对应的弧度值 (可选)
    feedback->angle_rad = (float)feedback->angle * 2.0f * 3.1415926535f / 8191.0f;

    // 计算实际电流值 (A) (可选)
    feedback->current_A = (float)feedback->current * 20.0f / 16384.0f;

    return motor_id;
}

/**
 * @brief CAN接收回调函数中调用解算函数
 */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    CAN_RxHeaderTypeDef header;
    uint8_t data[8];
    C620_Feedback_t feedback;

    if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &header, data) == HAL_OK)
    {
        // 如果发送的是电流数据，解算电流数据，用于模拟电控
        HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
        if (header.StdId == 0x200 || header.StdId == 0x1FF)
        {
            OLED_ShowString(0, 1, "Recv CAN Current Msg");
            OLED_ShowHexNumber(0, 2, header.StdId);
            OLED_ShowNumber(0, 3, (int16_t)((data[0] << 8) | data[1]));
            OLED_ShowNumber(0, 4, (int16_t)((data[2] << 8) | data[3]));
            OLED_ShowNumber(0, 5, (int16_t)((data[4] << 8) | data[5]));
            OLED_ShowNumber(0, 6, (int16_t)((data[6] << 8) | data[7]));

            printf("Received CAN Current Msg: ID=0x%03lX, Data=%d, %d, %d, %d\n", header.StdId,
                   (int16_t)((data[0] << 8) | data[1]), (int16_t)((data[2] << 8) | data[3]),
                   (int16_t)((data[4] << 8) | data[5]), (int16_t)((data[6] << 8) | data[7]));
            return;
        }

        uint8_t motor_id = C620_DecodeFeedback(header.StdId, data, &feedback); // 尝试结算电控发来的数据
        if (motor_id != 0)
        {
            // 将解算数据存储到全局数组中
            motor_feedback[motor_id - 1] = feedback;

            // 可选：打印调试信息
            printf("Motor ID%d: Angle=%d, Speed=%d rpm, Current=%.2fA, Temp=%d\n",
                   motor_id, feedback.angle, feedback.speed,
                   feedback.current_A, feedback.temperature);
            OLED_ShowString(0, 1, "Recv CAN Motor Msg");
            OLED_ShowString(0, 2, "id/ang,spe/,cur/temp");
            OLED_ShowNumber(0, 3, motor_id);
            OLED_ShowNumber(0, 4, feedback.angle);
            OLED_ShowNumber(0, 5, feedback.speed);
            OLED_ShowNumber(0, 6, feedback.current_A);
            OLED_ShowNumber(0, 7, feedback.temperature);
        }
        else
        {
            OLED_ShowString(0, 1, "Recv CAN Msg Err");
            OLED_ShowHexNumber(0, 2, header.StdId);
        }
    }
}

/**
 * @brief 获取指定电机的角度（弧度）
 */
static float C620_GetAngleRad(uint8_t motor_id)
{
    if (motor_id < 1 || motor_id > 8)
        return 0.0f;
    return motor_feedback[motor_id - 1].angle_rad;
}

/**
 * @brief 获取指定电机的转速（RPM）
 */
static int16_t C620_GetSpeed(uint8_t motor_id)
{
    if (motor_id < 1 || motor_id > 8)
        return 0;
    return motor_feedback[motor_id - 1].speed;
}

/**
 * @brief 获取指定电机的电流（A）
 */
static float C620_GetCurrent(uint8_t motor_id)
{
    if (motor_id < 1 || motor_id > 8)
        return 0.0f;
    return motor_feedback[motor_id - 1].current_A;
}

/**
 * @brief 获取指定电机的温度（°C）
 */
static int8_t C620_GetTemperature(uint8_t motor_id)
{
    if (motor_id < 1 || motor_id > 8)
        return 0;
    return motor_feedback[motor_id - 1].temperature;
}

// 错误处理回调
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan)
{
    // 错误处理代码
    // 可以重置CAN或进行其他错误恢复操作
}